//2025.8.22青浦演示视觉
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <atomic>
#include <thread>
#include <rclcpp/executors/multi_threaded_executor.hpp>
// #include "servo_visual/doc/examples/realtime_servo/src/visual_control.hpp"   // 修改：包含新创建的头文件
#include "/home/jetson/zc6/src/servo_visual/include/servo_visual/ibvs_control.hpp"
#include <std_msgs/msg/float64.hpp>
#include <algorithm>
#include <vector>
#include "path_plan/path1_waypoints.hpp"
// #define USE_IBVS  // 定义宏以启用 IBVS 功能

#ifdef USE_IBVS
#include "/home/jetson/zc6/src/servo_visual/doc/examples/realtime_servo/src/ibvs_control.hpp"
#else
#include "/home/jetson/zc6/src/servo_visual/doc/examples/realtime_servo/src/visual_control.hpp"   // 修改：包含新创建的头文件
#endif
using servo_visual::IBVSControl; 
class CombinedNode : public rclcpp::Node {
public:
    CombinedNode() : Node("combined_node"), force_sign(true), closest_index(0) {
        // 初始化力数据订阅
        force_sub_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/force_data", 10,
            std::bind(&CombinedNode::force_callback, this, std::placeholders::_1));

        // 初始化MoveGroupInterface
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            std::shared_ptr<rclcpp::Node>(this),  // 显式创建shared_ptr
            "zc4");
        
        RCLCPP_INFO(get_logger(), "节点初始化完成");
    }

    #ifdef USE_IBVS
    std::shared_ptr<servo_visual::IBVSControl> get_ibvs_control_node() {
        return ibvs_control_node;
    }
    #else
    std::shared_ptr<VisualControl> get_visual_control_node() {
        return visual_control_node_;
    }
    #endif

    // 修改 start_execution 以接受路径索引
    void start_execution(int path_index) {
        std::thread([this, path_index]() {
            switch (path_index) {
                case 1:
                    executeCartesianPath1();
                    #ifdef USE_IBVS
                        // 创建并初始化 IBVSControl 节点，启动 control_loop 定时器
                        ibvs_control_node = std::make_shared<IBVSControl>();
                        ibvs_control_node->initialize();
                        // 等待 IBVS 执行完毕
                        while(rclcpp::ok() && !ibvs_control_node->isFinished()){
                            RCLCPP_INFO(get_logger(), "IBVS 执行中...");
                            std::this_thread::sleep_for(std::chrono::milliseconds(100));
                        }
                        RCLCPP_INFO(get_logger(), "IBVS 执行完毕，开始返回 middle 位置");
                    #else
                        // 创建并初始化 VisualControl 节点，启动 control_loop 定时器
                        visual_control_node_ = std::make_shared<VisualControl>(/* 可传入必要参数 */);
                        visual_control_node_->initialize();
                        // 新增：等待 Visual 执行完毕，假定 VisualControl 有 isFinished() 方法
                        
                        while(rclcpp::ok() && !visual_control_node_->isFinished()){
                            RCLCPP_INFO(get_logger(), "Visual 执行中...");
                            std::this_thread::sleep_for(std::chrono::milliseconds(100));
                        }
                        RCLCPP_INFO(get_logger(), "Visual 执行完毕，开始返回 middle 位置");
                    #endif

                    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                    if (returnToMiddle()) {
                        RCLCPP_INFO(get_logger(), "成功返回 middle 位置");
                    } else {
                        RCLCPP_ERROR(get_logger(), "返回 middle 位置失败");
                    }
                    rclcpp::shutdown(); 
                    break;
                default:
                    RCLCPP_ERROR(get_logger(), "无效的路径索引: %d", path_index);
                    break;
            }
        }).detach();
    }

private:
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_sub_;
    std::atomic<bool> force_sign;
    std::atomic<size_t> closest_index;
    #ifdef USE_IBVS
        std::shared_ptr<servo_visual::IBVSControl> ibvs_control_node;
    #else
        std::shared_ptr<VisualControl> visual_control_node_;
    #endif

    void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
        if (std::abs(msg->twist.linear.z) >= 1.5) {
            force_sign = false;
        } else {
            force_sign = true;
        }
    }

    bool returnToMiddle() {
        // 等待500ms, 确保MoveGroup action client/server已就绪
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        move_group_->setNamedTarget("middle");
        moveit::planning_interface::MoveGroupInterface::Plan home_plan;
        bool success = (move_group_->plan(home_plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success) {
            move_group_->execute(home_plan);
            RCLCPP_INFO(get_logger(), "回到middle位置");
            return true;
        }
        RCLCPP_ERROR(get_logger(), "回到middle位置失败");
        return false;
    }

    bool continueCartesianPath(const std::vector<geometry_msgs::msg::Pose>& original_waypoints) {
        geometry_msgs::msg::Pose current_pose = move_group_->getCurrentPose().pose;
        std::vector<geometry_msgs::msg::Pose> remaining_waypoints;
        double min_distance = std::numeric_limits<double>::max();

        for (size_t i = 0; i < original_waypoints.size(); ++i) {
            double dx = current_pose.position.x - original_waypoints[i].position.x;
            double dy = current_pose.position.y - original_waypoints[i].position.y;
            double dz = current_pose.position.z - original_waypoints[i].position.z;
            double distance = std::sqrt(dx*dx + dy*dy + dz*dz);
            if (distance < min_distance) {
                min_distance = distance;
                closest_index = i;
            }
        }

        for (size_t i = closest_index + 1; i < original_waypoints.size(); ++i) {
            remaining_waypoints.push_back(original_waypoints[i]);
        }

        if (remaining_waypoints.empty()) return false;

        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan new_plan;
        double fraction = move_group_->computeCartesianPath(remaining_waypoints, 0.01, 0.0, new_plan.trajectory_);
        
        if (fraction >= 0.9) {
            move_group_->asyncExecute(new_plan);
            return true;
        }
        return false;
    }

    void executeCartesianPath1() {
        if (!returnToMiddle()) return;
        // 直接读取外部文件定义的路径
        std::vector<geometry_msgs::msg::Pose> waypoints = paths::path1_waypoints();

        move_group_->stop();
        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
        
        if (fraction < 0.9) {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return;
        }

        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        move_group_->asyncExecute(cartesian_plan);
        bool is_stopped = false;
        bool is_forced = false;
        std::atomic<bool> motion_complete(false);
        auto start_time = std::chrono::steady_clock::now();
        const std::chrono::seconds timeout(10); // 设置15秒超时

        while (rclcpp::ok() && !motion_complete && !is_forced) {
            auto elapsed_time = std::chrono::steady_clock::now() - start_time;
            if (elapsed_time > timeout) {
                RCLCPP_ERROR(get_logger(), "运动执行超时");
                motion_complete = true;
                break;
            }
            if (!force_sign.load()) {
                if (!is_stopped) {
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                    RCLCPP_INFO(get_logger(), "检测到过大力，尝试停止运动...");
                    move_group_->stop();
                    is_stopped = true;
                    RCLCPP_INFO(get_logger(), "运动已停止");
                }
            } else {
                if (is_stopped) {
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                    RCLCPP_INFO(get_logger(), "力信号恢复，尝试继续运动...");
                    if (continueCartesianPath(waypoints)) {
                        RCLCPP_INFO(get_logger(), "已恢复运动");
                        is_stopped = false;
                    } else {
                        RCLCPP_INFO(get_logger(), "无法继续运动或路径已完成");
                        motion_complete = true;
                        break;
                    }
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

        if (motion_complete.load()) {
            RCLCPP_INFO(get_logger(), "运动执行循环结束（可能已完成或无法继续）");
        } else if (!rclcpp::ok()) {
            RCLCPP_INFO(get_logger(), "运动执行循环因 rclcpp 关闭而结束");
        }
        move_group_->stop();
    }

};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CombinedNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    
    node->start_execution(1); 
    #ifdef USE_IBVS
    // 等待 IBVSControl 节点创建成功
    while (rclcpp::ok() && node->get_ibvs_control_node() == nullptr) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    executor.add_node(node->get_ibvs_control_node());
    #else
    // 等待 VisualControl 节点创建成功
    while (rclcpp::ok() && node->get_visual_control_node() == nullptr) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    executor.add_node(node->get_visual_control_node());
    #endif

    RCLCPP_INFO(node->get_logger(), "开始执行器 spin");
    executor.spin();

    RCLCPP_INFO(node->get_logger(), "执行器 spin 结束");
    rclcpp::shutdown();
    return 0;
}